﻿# coding=utf-8
# 2023/4/27: 首个记录，基于旧2020/4/1版本
# 2023/10/11: 补充部分英文注释

import bi_common as bi


class USoundSensorProperty:
    def __init__(self):
        self.origin_x = 0  # [m] X-coordinate of the n-th sensor's position / 传感器原点X坐标
        self.origin_y = 0  # [m] Y-coordinate of the n-th sensor's position / 传感器原点Y坐标
        self.orientation = 0  # Central heading angle of the n-th sensor / 传感器中心轴朝向
        self.angle_range_a = 0  # [°] α angle range (generally around 100°, with sensor's origin as the center of the circle) / α角范围（一般在100°左右，以OriginX/Y为圆心）
        self.angle_range_b = 0  # [°] β angle range (generally ranges 0~30°) / β角范围（一般在0~30°）
        self.distance_range_a = 0  # [m] Detect range corresponding to α angle (shorter) / α角对应的探测距离（较短）
        self.distance_range_b = 0  # [m] Detect range corresponding to β angle (longer) / β角对应的探测距离（较长）


class USoundSensorData:
    def __init__(self):
        self.id = 0  # Sensor ID, starting from 1 / 传感器ID，从1开始
        self.properties = USoundSensorProperty()  # Sensor properties / 超声波传感器属性
        self.distance_valid = False
        self.distance = 0.0  # [m] Detected barrier's distance, set "valid" field to False if no barrier / 障碍物的距离，无障碍物则valid字段设为False


class USoundSample:
    def __init__(self):
        self.time = 0.0  # [s] Time offset in session / Session内的相对时间
        self.data_list = []  # Data of each sensor / 各个超声波传感器的数据

    # Convert to general sample for output / 转通用样本，用于样本输出
    def to_general_sample(self):
        output = bi.agency.create_general_sample()
        output.protocol = 'usound-sample-v1'
        output.time = self.time
        output.significant = 1 + len(self.data_list) * 9
        output.values = []
        output.values.append(len(self.data_list))
        for data in self.data_list:
            output.values.append(data.id)
            output.values.append(data.distance if data.distance_valid else None)
            output.values.append(data.properties.origin_x)
            output.values.append(data.properties.origin_y)
            output.values.append(data.properties.orientation)
            output.values.append(data.properties.angle_range_a)
            output.values.append(data.properties.angle_range_b)
            output.values.append(data.properties.distance_range_a)
            output.values.append(data.properties.distance_range_b)
        return output


def _conv_usound_sample_v1(gs):
    values_count = len(gs.values)
    if values_count < 1:
        return None
    sensor_count = int(gs.values[0]) if gs.values[0] is not None else 0
    if values_count != 1 + 9 * sensor_count:
        return None
    output = USoundSample()
    output.time = gs.time
    for i in range(0, sensor_count):
        b = 1 + 9 * i
        if gs.values[b] is not None or gs.values[b + 2] is not None or gs.values[b + 3] is not None or gs.values[b + 4] is not None or gs.values[b + 5] is not None or gs.values[b + 6] is not None or gs.values[b + 7] is not None or gs.values[b + 8] is not None:
            data = USoundSensorData()
            data.id = int(gs.values[b])
            if gs.values[b + 1] is not None:
                data.distance_valid = True
                data.distance = float(gs.values[b + 1])
            data.properties.origin_x = float(gs.values[b + 2])
            data.properties.origin_y = float(gs.values[b + 3])
            data.properties.orientation = float(gs.values[b + 4])
            data.properties.angle_range_a = float(gs.values[b + 5])
            data.properties.angle_range_b = float(gs.values[b + 6])
            data.properties.distance_range_a = float(gs.values[b + 7])
            data.properties.distance_range_b = float(gs.values[b + 8])
            output.data_list.append(data)
    return output


def _interpolate_usound_sample(s1, w1, s2, w2):
    output = USoundSample()
    output.time = bi.time
    for o1 in s1.data_list:
        id = o1.id
        for o2 in s2.data_list:
            if o2.id != id:
                continue
            data = USoundSensorData()
            data.id = id
            data.properties = o1.properties
            if o1.distance_valid and o2.distance_valid:
                data.distance_valid = True
                data.distance = o1.distance * w1 + o2.distance * w2
            output.data_list.append(data)
    return output


# Query USoundSample for input / 获取USoundSample，用于样本输入
def get_usound_sample():
    s1 = None
    s2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id = 'usound-sample-v1'
    if protocol_id in bi.input_samples:
        pair = bi.input_samples[protocol_id]
        s1 = _conv_usound_sample_v1(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_usound_sample_v1(pair.sample2)
        w2 = pair.weight2
    if s1 is not None and s2 is not None:
        return _interpolate_usound_sample(s1, w1, s2, w2)
    return None
